#!/bin/sh
#
# @name Aion Robotics R1 UGV
#
# @url http://docs.aionrobotics.com/en/latest/r1-ugv.html
#
# @type Rover
# @class Rover
#
# @board px4_fmu-v2 exclude
#
# @output MAIN0 Speed of left wheels
# @output MAIN1 Speed of right wheels
#
# @maintainer Timothy Scott
#
# @board px4_fmu-v2 exclude
# @board intel_aerofc-v1 exclude
# @board bitcraze_crazyflie exclude
#

. ${R}etc/init.d/rc.rover_defaults

if [ $AUTOCNF = yes ]
then
	param set BAT_N_CELLS 4

	param set EKF2_GBIAS_INIT 0.01
	param set EKF2_ANGERR_INIT 0.01
	param set EKF2_MAG_TYPE 1

	param set FW_AIRSPD_MIN 0
	param set FW_AIRSPD_TRIM 1
	param set FW_AIRSPD_MAX 3

	param set GND_SP_CTRL_MODE 1
	param set GND_L1_DIST 5
	param set GND_L1_PERIOD 3
	param set GND_THR_IDLE 0
	param set GND_THR_CRUISE 0.7
	param set GND_THR_MAX 0.5

	# Because this is differential drive, it can make a turn with radius 0.
	# This corresponds to a turn angle of pi radians.
	# If a special case is made for differential-drive, this will need to change.
	param set GND_MAX_ANG 3.142
	param set GND_WHEEL_BASE 0.3

	# TODO: Set to -1.0, to allow reversing. This will require many changes in the codebase
	# to support negative throttle.
	param set GND_THR_MIN 0
	param set GND_SPEED_P 0.25
	param set GND_SPEED_I 3
	param set GND_SPEED_D 0.001
	param set GND_SPEED_IMAX 0.125
	param set GND_SPEED_THR_SC 1

	param set MIS_LTRMIN_ALT 0.01
	param set MIS_TAKEOFF_ALT 0.01

	param set NAV_ACC_RAD 0.5

	# Provide ESC a constant 1500 us pulse, which corresponds to
	# idle on the Roboclaw motor controller on the Aion R1
	param set PWM_DISARMED 1500
	param set PWM_MAIN_DIS0 1500
	param set PWM_MAIN_DIS1 1500
	param set PWM_MAX 2000
	param set PWM_MIN 1000

	# Enable Airspeed check circuit breaker because Rovers will have no airspeed sensor
	param set CBRK_AIRSPD_CHK 162128

	# Differential drive acts like ackermann steering with a maximum turn angle of 180 degrees, or pi radians
	param set GND_MAX_ANG 3.1415

	param set RBCLW_BAUD 8
	param set RBCLW_COUNTS_REV 1200
	param set RBCLW_ADDRESS 128
	# param set SER_TEL4_BAUD 115200
	# 104 corresponds to Telem 4
	param set RBCLW_SER_CFG 104
fi

# Start this driver after setting parameters, because the driver uses some of those parameters.
# roboclaw start /dev/ttyS3

# Configure this as rover
set MAV_TYPE 10

# Set mixer
set MIXER generic_diff_rover

set PWM_MAIN_REV2 1
